#include "MeasurE452AlgFrontSe1ToSe9Edge.h"

MeasurE452AlgFrontSE1toSE9Edge::MeasurE452AlgFrontSE1toSE9Edge(int i):MeasurE452AlgBase(i)
{

}

void MeasurE452AlgFrontSE1toSE9Edge::operator()(cv::Mat &srcImg,
                                                int id,std::map<string, float> &measRes,
                                                int fixtureId)
{
    qDebug() << "start to E452 alg front se1 to se9 edge";

    std::vector<std::vector<cv::Point2f>> resptVec;
    resptVec.resize(9);
    for(auto i=26;i<35;i++)
    {
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-26,j));
        }
        //paras.push_back(tparas);
        std::vector<cv::Rect> rangVec;

        if(srcImg.empty()) {
            Q_ASSERT(0);
        }

        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> contours;
        MeasurE452EdgeDetect obj;
        obj.NewKirschEdgeOuter(imgt,tparas,contours);
        ImageCoordinateTransformation(contours,rangVec);

        std::unique_lock<std::mutex> lock(*MeasurE452AlgBase::mtxptrVec[id]);
        while (!MeasurE452AlgBase::flagVec[id])
        {
            (*MeasurE452AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            resptVec[i-26].push_back(ptsOut);
        }
    }

    // go on coding
    //cout<<resptVec[0][0].x<<"\t"<<resptVec[5][0].y<<endl;
    vector<float> sedata(9,0);
    for(int i=0;i<resptVec[0].size();++i){
        sedata[0]+=resptVec[0][i].y;
    }
    sedata[0]/=resptVec[0].size();
    for(int i=0;i<resptVec[1].size();++i){
        sedata[1]+=resptVec[1][i].y;
    }
    sedata[1]/=resptVec[1].size();
    for(int i=0;i<resptVec[2].size();++i){
        sedata[2]+=resptVec[2][i].x;
    }
    sedata[2]/=resptVec[2].size();
    for(int i=0;i<resptVec[3].size();++i){
        sedata[3]+=resptVec[3][i].y;
    }
    sedata[3]/=resptVec[3].size();
    for(int i=0;i<resptVec[4].size();++i){
        sedata[4]+=resptVec[4][i].x;
    }
    sedata[4]/=resptVec[4].size();
    for(int i=0;i<resptVec[5].size();++i){
        sedata[5]+=resptVec[5][i].y;
    }
    sedata[5]/=resptVec[5].size();
    for(int i=0;i<resptVec[6].size();++i){
        sedata[6]+=resptVec[6][i].x;
    }
    sedata[6]/=resptVec[6].size();
    for(int i=0;i<resptVec[7].size();++i){
        sedata[7]+=resptVec[7][i].y;
    }
    sedata[7]/=resptVec[7].size();
    for(int i=0;i<resptVec[8].size();++i){
        sedata[8]+=resptVec[8][i].x;
    }
    sedata[8]/=resptVec[8].size();

    measRes.insert(std::make_pair(string("FAI4-SE2SE6-distance_pixel"), fabs(sedata[1]-sedata[5])));
    float fai4se = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI4-SE2SE6-distance")], fabs(sedata[1]-sedata[5]));
    measRes.insert(std::make_pair(string("FAI4-SE2SE6-distance_real"), fai4se));

    measRes.insert(std::make_pair(string("FAI5-SE3SE9-distance_pixel"), fabs(sedata[2]-sedata[8])));
    float fai5se = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI5-SE3SE9-distance")], fabs(sedata[2]-sedata[8]));
    measRes.insert(std::make_pair(string("FAI5-SE3SE9-distance_real"), fai5se));

    measRes.insert(std::make_pair(string("FAI6-SE7-distance_pixel"), sedata[6]));
    float fai6se = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI6-SE7-distance")], sedata[6]);
    measRes.insert(std::make_pair(string("FAI6-SE7-distance_real"), fai6se));

    measRes.insert(std::make_pair(string("FAI7-SE5-distance_pixel"), sedata[4]));
    float fai7se = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI7-SE5-distance")], sedata[4]);
    measRes.insert(std::make_pair(string("FAI7-SE5-distance_real"), fai7se));

    measRes.insert(std::make_pair(string("FAI8-SE1SE8-distance_pixel"), fabs(sedata[0]-sedata[7])));
    float fai8se = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI8-SE1SE8-distance")], fabs(sedata[0]-sedata[7]));
    measRes.insert(std::make_pair(string("FAI8-SE1SE8-distance_real"), fai8se));

    measRes.insert(std::make_pair(string("FAI9-SE4-distance_pixel"), sedata[3]));
    float fai9se = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI9-SE4-distance")], sedata[3]);
    measRes.insert(std::make_pair(string("FAI9-SE4-distance_real"), fai9se));

    qDebug() << "end to E452 alg front se1 to se9 edge";
}
